import sys
import rclpy
from rclpy.node import Node
from go2_tutorial_inter.srv import Cruising

# 自定义节点类
class CruClient(Node):
    def __init__(self):
        super().__init__('cru_client_node_py')
        # 创建客户端
        self.client = self.create_client(Cruising, 'cruising')
        self.get_logger().info("客户端创建，等待连接服务端！")

    def connect_server(self):
        # 连接服务端
        while not self.client.wait_for_service(timeout_sec=1.0):
            if not rclpy.ok():
                self.get_logger().error("Interrupted while waiting for the service. Exiting.")
                return False
            self.get_logger().info("服务连接中，请稍候...")
        return True

    def send_request(self, flag):
        # 发送请求
        request = Cruising.Request()
        request.flag = flag
        return self.client.call_async(request)

def main(args=None):
    # 处理通过终端提交的数据
    if len(sys.argv) != 2:
        print("请提交一个整型数据！")
        return 1

    # 初始化ROS2客户端
    rclpy.init(args=args)

    # 创建自定义类对象，并调用连接服务以及请求发送请求的函数
    client = CruClient()

    # 连接服务端
    if not client.connect_server():
        client.get_logger().info("服务连接失败!")
        return 0

    # 发送请求
    future = client.send_request(int(sys.argv[1]))

    # 处理响应结果
    rclpy.spin_until_future_complete(client, future)
    if future.done():
        try:
            response = future.result()
            client.get_logger().info("请求正常处理")
            client.get_logger().info(f"响应坐标: (%.2f,%.2f)" % (response.point.x, response.point.y))
        except Exception as e:
            client.get_logger().error(f"请求异常: {e}")
    else:
        client.get_logger().info("请求未完成")

    # 资源释放
    client.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()